#include "firmware.h"

const char HEX_DIGITS[] = "0123456789ABCDEF";


// Polynomial table x^8 + x^5 + x^4 + 1 - used in Dallas
// Semiconductor iButton devices (1-wire)
static const unsigned char PolynomialTable[] =
{
   0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83, 
   0xc2, 0x9c, 0x7e, 0x20, 0xa3, 0xfd, 0x1f, 0x41, 
   0x9d, 0xc3, 0x21, 0x7f, 0xfc, 0xa2, 0x40, 0x1e, 
   0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc, 
   0x23, 0x7d, 0x9f, 0xc1, 0x42, 0x1c, 0xfe, 0xa0, 
   0xe1, 0xbf, 0x5d, 0x03, 0x80, 0xde, 0x3c, 0x62, 
   0xbe, 0xe0, 0x02, 0x5c, 0xdf, 0x81, 0x63, 0x3d, 
   0x7c, 0x22, 0xc0, 0x9e, 0x1d, 0x43, 0xa1, 0xff, 
   0x46, 0x18, 0xfa, 0xa4, 0x27, 0x79, 0x9b, 0xc5, 
   0x84, 0xda, 0x38, 0x66, 0xe5, 0xbb, 0x59, 0x07, 
   0xdb, 0x85, 0x67, 0x39, 0xba, 0xe4, 0x06, 0x58, 
   0x19, 0x47, 0xa5, 0xfb, 0x78, 0x26, 0xc4, 0x9a, 
   0x65, 0x3b, 0xd9, 0x87, 0x04, 0x5a, 0xb8, 0xe6, 
   0xa7, 0xf9, 0x1b, 0x45, 0xc6, 0x98, 0x7a, 0x24, 
   0xf8, 0xa6, 0x44, 0x1a, 0x99, 0xc7, 0x25, 0x7b, 
   0x3a, 0x64, 0x86, 0xd8, 0x5b, 0x05, 0xe7, 0xb9, 
   0x8c, 0xd2, 0x30, 0x6e, 0xed, 0xb3, 0x51, 0x0f, 
   0x4e, 0x10, 0xf2, 0xac, 0x2f, 0x71, 0x93, 0xcd, 
   0x11, 0x4f, 0xad, 0xf3, 0x70, 0x2e, 0xcc, 0x92, 
   0xd3, 0x8d, 0x6f, 0x31, 0xb2, 0xec, 0x0e, 0x50, 
   0xaf, 0xf1, 0x13, 0x4d, 0xce, 0x90, 0x72, 0x2c, 
   0x6d, 0x33, 0xd1, 0x8f, 0x0c, 0x52, 0xb0, 0xee, 
   0x32, 0x6c, 0x8e, 0xd0, 0x53, 0x0d, 0xef, 0xb1, 
   0xf0, 0xae, 0x4c, 0x12, 0x91, 0xcf, 0x2d, 0x73, 
   0xca, 0x94, 0x76, 0x28, 0xab, 0xf5, 0x17, 0x49, 
   0x08, 0x56, 0xb4, 0xea, 0x69, 0x37, 0xd5, 0x8b, 
   0x57, 0x09, 0xeb, 0xb5, 0x36, 0x68, 0x8a, 0xd4, 
   0x95, 0xcb, 0x29, 0x77, 0xf4, 0xaa, 0x48, 0x16, 
   0xe9, 0xb7, 0x55, 0x0b, 0x88, 0xd6, 0x34, 0x6a, 
   0x2b, 0x75, 0x97, 0xc9, 0x4a, 0x14, 0xf6, 0xa8, 
   0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, 0xf7, 
   0xb6, 0xe8, 0x0a, 0x54, 0xd7, 0x89, 0x6b, 0x35,
};




// message output routines

void PutHex(int Byte)
{   
   uart_putc(HEX_DIGITS[0x0F & (Byte >> 4)]);
   uart_putc(HEX_DIGITS[0x0F &  Byte      ]);
}

// Message
static char FirstParamFlag = 1;
static char CompressFlag = 0;  
static int  CompressChar = 0;    
static int  CompressCount = -1;
static unsigned char StaticCrc;
//---------------------------------------------------------------------------
// MsgOpen - Start new message
//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
void MsgOpen(char Cmd)
{
   uart_putc('[');
   uart_putc(Cmd);
   
   FirstParamFlag = 1;    
   CompressFlag   = 0;        
   StaticCrc      = 0;
}

//---------------------------------------------------------------------------
// MsgCompress - Enable compressed message stream
//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
void MsgCompress()
{    
   // enable compression   
   uart_putc(0xFE); // compression start sequence
   uart_putc(0xFD);
   uart_putc(0x00);   
   
   CompressFlag = 1;
   CompressCount  = 0;  // no chars remaining.   
}            

static void MsgFlushCompression()
{
   
   if (CompressCount == 0) return; // no prev sequence
      
   // Clear prev sequences
   if (CompressChar != 0xFE) {
      // normal sequence.
      if (CompressCount > 3) {
         // output sequence
         uart_putc(0xFE);
         uart_putc(CompressCount);
         uart_putc(CompressChar);
      } else {
         // output loose chars
         for (int i=0; i < CompressCount; i++) {
            uart_putc(CompressChar);
         }
      }
   } else {
      // special sequence
      if (CompressCount > 1) {
         // output sequence
         uart_putc(0xFE);
         uart_putc(0xFD);
         uart_putc(CompressCount);
      } else {
         // output loose chars
         for (int i=0; i < CompressCount; i++) {
            uart_putc(0xFE);
            uart_putc(0xFF);
         }
      }
   }
}

void MsgParamNoDot(unsigned long Dword)
{ 
   if ((Dword > 0x0000FF) &&  (CompressFlag == 0))
      MsgParamNoDot(Dword >> 8);

   StaticCrc = PolynomialTable[(Dword & 0xFF) ^ StaticCrc];
   
   if (CompressFlag) {
      // output in compressed mode
      if (CompressCount == 0) {
         // no chars stored yet
         CompressCount = 1;
         CompressChar = (unsigned char) Dword;
         return;
      }         
      if (CompressChar == (unsigned char)  Dword) {
         // same char as before
         CompressCount ++;
         if (CompressCount >= 252) {
            // max number of chars in one sequence.
            MsgFlushCompression();
            CompressCount = 0;
         }
         return;
      }
      // Different char
      MsgFlushCompression();
      CompressCount = 1;
      CompressChar = (unsigned char) Dword;  
   } else {
      // output in ascii mode
      PutHex(Dword);  
   }

}
          
//---------------------------------------------------------------------------
// MsgParam - Send a param as part of a message.
//---------------------------------------------------------------------------          
// Note: normal, Param up to 32 bits are supported.
//       In compressed mode, only 8 byte params are
//       supported.                
// Note2: only the relevant bytes (the highest byte which is non-zero and all 
//       lower bytes) are output and used in CRC calculation.
//---------------------------------------------------------------------------
void MsgParam(unsigned long Dword) 
{
   if ((FirstParamFlag == 0) && (CompressFlag == 0)) uart_putc('.');
   FirstParamFlag = 0;

   MsgParamNoDot(Dword);
}

//---------------------------------------------------------------------------
// MsgClose - Finish message
//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
void MsgClose()
{ 

   if (CompressFlag) {     
      MsgFlushCompression();      
      uart_putc(0xFE); // compression end sequence
      uart_putc(0xFD);
      uart_putc(0x01);
   }


   uart_putc('*');
   PutHex(StaticCrc);   
   uart_putc(']');
   uart_putc('\r');
   uart_putc('\n');
}
                                       
//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
//---------------------------------------------------------------------------                                       
int SerialReceiveFSM(char InByte)
{  static int state = 0;
   static int ParamIndex;

   switch(state) {
      case 0:  // idle   
         ReceiveMsgParamCount = -1;
         if (InByte == '[') state = 1;
      break;
         
      case 1:  // wait for command
         if (InByte == ']') { // unexpected end
            state = 0;
            break;
         }
         ReceiveMsgCmd = InByte;
         ParamIndex = 0;
         ReceiveMsgParam[ParamIndex] = 0;
         state = 2;
      break;
      
      case 2: // wait for first param or closing tag
         if (InByte == ']') { // end of message
            ReceiveMsgParamCount = 0;
            state = 0;
            return 1;      
         }
         // call state machine recursive to handle received char
         state = 3;
         return SerialReceiveFSM(InByte);
      break;

      case 3: // receive params
         if ((InByte >= '0') && (InByte <= '9')) {
            ReceiveMsgParam[ParamIndex] = 
               ReceiveMsgParam[ParamIndex] * 16 + InByte - '0';            
            break;
         }                                                                  
         
         if ((InByte >= 'a') && (InByte <= 'f')) InByte &= 0x5F;  // toupper
         
         if ((InByte >= 'A') && (InByte <= 'F')) {
            ReceiveMsgParam[ParamIndex] = 
               ReceiveMsgParam[ParamIndex] * 16 + InByte - 'A' + 10;            
            break;
         }
         if (InByte == '.') { // next param
            ParamIndex++;
            ReceiveMsgParam[ParamIndex] = 0;
            break;      
         }
         if (InByte == ']') { // end of message
            ParamIndex++;
            ReceiveMsgParam[ParamIndex] = 0;
            ReceiveMsgParamCount = ParamIndex;
            state = 0;
            return 1;      
         }
         // unexpected char
         state = 0;
      break;
         
      default :
         state = 0;
      break;
   }       
   return 0;
}

//------------------------------------------------------------------------
// SerialReceiveTakt - Read chars from serial queue and decode msg
//------------------------------------------------------------------------
// Msg are structured like:
//    [Ahh.hh.hh]
//    Where
//    [ and ] are opening and closing tags
//    A = single byte command
//    hh (optional) first parameter in 2-byte hex format (0..9,A-F)
//    .hh (optional) subsequent parameters
//
// return: normal 0, 1 if message is complete
// (ReceiveMsgParamCount is -1 during receive and >= 0 when done)
//------------------------------------------------------------------------
int SerialReceiveTakt()
{
   unsigned int InChar;
   
   for (;;) {
   
      InChar = uart_getc();

      // check if there is data to process      
      if (InChar == UART_NO_DATA) return 0;

      if (SerialReceiveFSM(InChar)) {
         return 1;
      }
   }  
   return 0;
}

